#include <std_msgs/Int32.h>

#include <matchmaker/client.h>

/**
 * @brief Example of advertising a service, with changing status
 */
class ExampleProvider
{
public:
  ExampleProvider()
  {
    count_ = 1;
  }

  ~ExampleProvider() { }

  void main()
  {
    // Create a random, changing topic name, for testing.
    std::stringstream ss;
    struct timeval timeNow;
    gettimeofday(&timeNow, NULL);
    ss << "test_" << timeNow.tv_usec;
    std::string topicName = ss.str();

    // Create the ROS-level entity (message publisher, in this case).
    myServicePub_ = nh_.advertise<std_msgs::Int32>(ss.str(), 1);

    // Advertise the service with the matchmaker library.
    myServiceAd_ = mmClient_.advertiseService("MadeUpService", myServicePub_);
    // Start with available status.
    myServiceAd_.setStatus(matchmaker::ServiceStatus::AVAILABLE);

    // Start a timer to update the service status, and send data messages.
    myServiceTimer_ = nh_.createTimer(ros::Duration(1),
        &ExampleProvider::myServiceTimerCB, this);

    // Spin to process ROS events, until killed.
    ros::spin();
  }

private:
  void myServiceTimerCB(const ros::TimerEvent& event)
  {
    // Set service status to available for 20 seconds in every 30, for testing.
    if ((count_ % 30) < 20) {
      myServiceAd_.setStatus(matchmaker::ServiceStatus::AVAILABLE);

      ROS_INFO_STREAM("Publishing msg value " << count_ << " on " << myServicePub_.getTopic());
      std_msgs::Int32 msg;
      msg.data = count_;
      myServicePub_.publish(msg);
    }
    else {
      // Set service status to unmet dependencies, to test matchmaker.
      myServiceAd_.setStatus(matchmaker::ServiceStatus::UNMET_DEPENDENCIES, "missing widget");

      ROS_INFO_STREAM("Simulating service dependency problem.");
    }
    count_++;
  }

  ros::NodeHandle nh_;

  matchmaker::Client mmClient_;

  matchmaker::ServiceAdvertiser myServiceAd_;
  ros::Publisher myServicePub_;
  ros::Timer myServiceTimer_;

  int count_;
};

int main(int argc, char ** argv)
{
  // AnonymousName init option adds changeable number to node name, so
  // we can run multiple instances of this node at once without clashes.
  ros::init(argc, argv, "provider", ros::init_options::AnonymousName);

  ExampleProvider example;
  example.main();
  exit(0);
}

/** \example example_provider.cpp
 * This example code shows how you might advertise a simple ROS message
 * publishing service using the matchmaker::Client. You can find it within
 * the matchmaker package, in the file src/example_provider.cpp.
 * Once you have built the matchmaker module, you can try running it like
 * this:
\verbatim
$ rosrun matchmaker example_publisher
\endverbatim
 *
 * Note the use of setStatus in the method myServiceTimerCB. The example
 * code simulates service failure for 10 seconds out of every 30, to show
 * what happens in the matchmaker and example_consumer.
 *
 * If you try running more than one example_publisher at once, you'll see
 * how an example_consumer will switch between them as they change state.
 */
